#ifndef ATTITUDE_CONTROLLER_H
#define ATTITUDE_CONTROLLER_H

#include "common.h"
#include "quaternion.h"
#include "vector3d.h"

// 姿态控制模式
typedef enum {
    ATTITUDE_MODE_IDLE,
    ATTITUDE_MODE_POINTING,
    ATTITUDE_MODE_SLEWING,
    ATTITUDE_MODE_STABILIZE
} AttitudeMode;

// PID控制器
typedef struct {
    double kp, ki, kd;
    double integral[3];
    double prev_error[3];
    double integral_limit;
    double output_limit;
} PID_Controller;

// 姿态控制器
typedef struct {
    Quaternion current_attitude;
    Quaternion target_attitude;
    Vector3D current_angular_velocity;
    Vector3D target_angular_velocity;
    
    PID_Controller pid_roll;
    PID_Controller pid_pitch;
    PID_Controller pid_yaw;
    
    AttitudeMode mode;
    double control_torque[3];
    double control_frequency;
    bool is_active;
} AttitudeController;

// 函数声明
ErrorCode attitude_controller_init(AttitudeController* controller);
ErrorCode attitude_controller_set_target(AttitudeController* controller, Quaternion target);
ErrorCode attitude_controller_update(AttitudeController* controller, double dt);
ErrorCode attitude_controller_get_torque(AttitudeController* controller, double torque[3]);
ErrorCode attitude_controller_set_mode(AttitudeController* controller, AttitudeMode mode);

// PID控制器函数
ErrorCode pid_init(PID_Controller* pid, double kp, double ki, double kd);
ErrorCode pid_update(PID_Controller* pid, double error, double dt, double* output);
ErrorCode pid_reset(PID_Controller* pid);

// 姿态计算函数
Quaternion attitude_optimal_rotation(Quaternion from, Quaternion to);
Vector3D attitude_error_vector(Quaternion current, Quaternion target);
double attitude_error_angle(Quaternion current, Quaternion target);

// 工具函数
void attitude_print_status(const AttitudeController* controller);

#endif // ATTITUDE_CONTROLLER_H
